// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PositioningPrecisionCalculator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using MicrosoftResearch.Infer.Distributions;
    using Test.Robotics.Metrics;

    /// <summary>
    /// Agent that compares the robot positioning estimate from the navigation pipeline with an external ground truth source.
    /// </summary>    
    [DataContract]
    public class PositioningPrecisionCalculator : SynchronizedConsumerProducerAgent<SLAM2DPoseMessage, GroundTruthMessage, MetricsResultMessage>, ISubscriptionReceiver<CompletionMessage>
    {
        /// <summary>
        /// The maximum discrepancy between slam and ground truth originating times in milliseconds.
        /// For external datasets, this value could be 1. 
        /// </summary>
        public const int DefaultSyncTolerance = 100;

        /// <summary>
        /// The name of the slam agent we measure
        /// </summary>
        [DataMember(Name = "SlamProducer")]
        private readonly string slamPoseProducerName;

        /// <summary>
        /// The name of the ground truth agent we compare with
        /// </summary>
        [DataMember(Name = "GroundTruthProducer")]
        private readonly string groundTruthProducerName;

        /// <summary>
        /// The name of the agent that sends a completion message
        /// </summary>
        [DataMember(Name = "CompletionProducer")]
        private readonly string completionProducerName;

        /// <summary>
        /// The set of estimated poses that represent the origin for future ground truth messages.
        /// </summary>
        private Dictionary<long, Tuple<Pose2D, GroundTruthMessage>> referencedPoses;

        /// <summary>
        /// The id of the iteration that generated this message. Used to tie all messages from the same run together.
        /// </summary>
        private long runId;

        /// <summary>
        /// Last ground truth pose Id (used to at the end of a run to calculate loop close entry)
        /// </summary>
        private long lastGroundTruthPoseId;

        /// <summary>
        /// The second robot pose message received (used to at the end of a run to calculate loop close entry)
        /// </summary>
        private SLAM2DPoseMessage secondSlamMessage;

        /// <summary>
        /// The second ground truth message received (used to at the end of a run to calculate loop close entry)
        /// </summary>
        private GroundTruthMessage secondGroundTruthMessage;

        /// <summary>
        /// Initializes a new instance of the PositioningPrecisionCalculator class.
        /// </summary>
        /// <param name="name">The name of the calculator agent</param>
        /// <param name="slamPoseProducer">The name of the slam agent producing the measured data</param>
        /// <param name="groundTruthProducer">The name of the ground truth agent we compare with</param>
        /// <param name="completionProducer">The name of the agent that indicates the data is complete</param>
        /// <param name="syncTolerance">Sync tolerance between slam and ground truth in milliseconds</param>
        public PositioningPrecisionCalculator(
            string name,
            IProducer<SLAM2DPoseMessage> slamPoseProducer,
            IProducer<GroundTruthMessage> groundTruthProducer,
            IProducer<CompletionMessage> completionProducer,
            int syncTolerance = DefaultSyncTolerance)
            : base(name, syncTolerance, true) 
        {
            this.slamPoseProducerName = slamPoseProducer.Name;
            this.groundTruthProducerName = groundTruthProducer.Name;
            this.completionProducerName = completionProducer.Name;
        }

        /// <summary>
        /// The name of the slam agent we measure
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.slamPoseProducerName; }
        }

        /// <summary>
        /// The name of the ground truth agent we compare with
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.groundTruthProducerName; }
        }

        /// <summary>
        /// De-facto constructor for the class, since the actual constructor is not called when the instance is created through deserialization.
        /// </summary>
        /// <param name="locator">The locator to use to find other agents.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            IAgentHost completionAgent = locator.GetAgent(this.completionProducerName);
            completionAgent.Subscribe<CompletionMessage>(this);
        }
        
        /// <summary>
        /// Initializes the agent on activation. This acts as a constructor because the agent is activated/deactivated multiple times.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.referencedPoses = new Dictionary<long, Tuple<Pose2D, GroundTruthMessage>>();
            this.runId = AgentMessage.GetCurrentTime();
        }

        /// <summary>
        /// Called whenever the completion agent is finished and publishes it's completion message
        /// Creates a fake loop closure segment for metrics and then forwards a completion message
        /// </summary>
        /// <param name="message">A completion message indicating that a file player finished playing.</param>
        public void Receive(CompletionMessage message)
        {
            // create a ground truth message that is a continuation of the previous one.
            GroundTruthMessage groundTruthMessage = new GroundTruthMessage(-1, this.lastGroundTruthPoseId, this.secondGroundTruthMessage.DeltaPose, AgentMessage.GetCurrentTime());

            // generate metrics for this "pretend" segment that represents travel from the current position to the second waypoint
            this.ProcessSlamAndGroundTruthMessages(this.secondSlamMessage, groundTruthMessage, true);
        }

        /// <summary>
        /// Computes the error between the estimated motion and the ground truth (actual) using the Freiberg technique.
        /// </summary>
        /// <param name="startPose">The start pose</param>
        /// <param name="endPose">The end pose</param>
        /// <param name="groundTruth">The ground truth message</param>
        /// <returns>Pair of errors for position and rotation</returns>
        internal static Tuple<double, double> ComputeError(Pose2D startPose, Pose2D endPose, GroundTruthMessage groundTruth)
        {
            // For distance error, project the actual travel deltas (according to SLAM) into the starting pose of the robot
            // then compare that to the expected deltas from the ground truth file.
            double slamDx = endPose.X - startPose.X;
            double slamDy = endPose.Y - startPose.Y;
            double slamLength = Math.Sqrt((slamDx * slamDx) + (slamDy * slamDy));
            double travelAngle = Math.Atan2(slamDy, slamDx);
            double projectedAngle = travelAngle - startPose.Heading;
            double projectedDx = slamLength * Math.Cos(projectedAngle);
            double projectedDy = slamLength * Math.Sin(projectedAngle);
            double errorX = groundTruth.DeltaPose.X - projectedDx;
            double errorY = groundTruth.DeltaPose.Y - projectedDy;

            return new Tuple<double, double>(Math.Sqrt(errorX * errorX + errorY * errorY), Math.Abs(Pose2D.ClampAngle(Math.Abs(groundTruth.DeltaPose.Heading - (endPose.Heading - startPose.Heading)))));
        }

        /// <summary>
        /// Computes the error between the estimated motion and the ground truth (actual) using the 3-point method.
        /// The 3-point method uses three consecutive slam points and the three associated ground truth points.
        /// (since all ground truth poses are relative, the first ground truth pose is (0, 0, 0))
        /// To calculate the error, we must align the ground truth and SLAM frames of reference.
        /// We use the first two segments (the tails) of the two pairs of segments as the means of alignment.
        /// </summary>
        /// <param name="slam1">Pose one of the three slam poses</param>
        /// <param name="slam2">Pose two of the three slam poses</param>
        /// <param name="slam3">Pose three of the three slam poses</param>
        /// <param name="groundTruth2">Delta pose two of the three ground truth poses</param>
        /// <param name="groundTruth3">Delta pose three of the three ground truth poses</param>
        /// <returns>Pair of errors for position and rotation</returns>
        internal static Tuple<double, double> ComputeJalobeanuError(Pose2D slam1, Pose2D slam2, Pose2D slam3, Pose2D groundTruth2, Pose2D groundTruth3)
        {
            // Since all ground truth poses are deltas (relative) ground truth pose one is always (0, 0, 0).
            Pose2D groundTruth1 = new Pose2D(0, 0, 0);

            // Ground truth poses are delta poses from the previous pose in the reference frame of the previous pose
            // find the difference between the tail angles of the slam and the ground truth one
            double angle = groundTruth1.Heading - Math.Atan2(slam2.Y - slam1.Y, slam2.X - slam1.X);

            // rotate slam point 3 around slam point 2 by the difference between the tail angles of the slam and ground truth 
            double slam3RotatedX = Math.Cos(angle) * (slam3.X - slam2.X) - Math.Sin(angle) * (slam3.Y - slam2.Y) + slam2.X;
            double slam3RotatedY = Math.Sin(angle) * (slam3.X - slam2.X) + Math.Cos(angle) * (slam3.Y - slam2.Y) + slam2.Y;

            // find slam deltas for the second segment
            double slamDx = slam3RotatedX - slam2.X;
            double slamDy = slam3RotatedY - slam2.Y;

            // convert external ground truth format (deltas relative to previous heading) into a length and angle
            double groundTruthSegment3Length = Math.Sqrt((groundTruth3.X * groundTruth3.X) + (groundTruth3.Y * groundTruth3.Y));
            double groundTruthSegment1AngleInGroundTruth1ReferenceFrame = Math.Atan2(groundTruth2.Y, groundTruth2.X);
            double groundTruthSegment2AngleInGroundTruth1ReferenceFrame = Math.Atan2(groundTruth3.Y, groundTruth3.X);
            double angleBetweenGroundTruthSegment2AndGroundTruthSegment1 = groundTruthSegment2AngleInGroundTruth1ReferenceFrame - groundTruthSegment1AngleInGroundTruth1ReferenceFrame + groundTruth2.Heading;

            // project ground truth into slam coordinates
            double groundTruthDxInSlamReferenceFrame = groundTruthSegment3Length * Math.Cos(angleBetweenGroundTruthSegment2AndGroundTruthSegment1);
            double groundTruthDyInSlamReferenceFrame = groundTruthSegment3Length * Math.Sin(angleBetweenGroundTruthSegment2AndGroundTruthSegment1);

            double errorX = slamDx - groundTruthDxInSlamReferenceFrame;
            double errorY = slamDy - groundTruthDyInSlamReferenceFrame;

            return new Tuple<double, double>(Math.Sqrt((errorX * errorX) + (errorY * errorY)), Math.Abs(Pose2D.ClampAngle(slam3.Heading - slam2.Heading - groundTruth3.Heading)));
        }

        /// <summary>
        /// Called when a pair of messages becomes available
        /// </summary>
        /// <param name="slamPoseMessage">The message containing the measured data</param>
        /// <param name="groundTruthMessage">The ground truth message we compare with</param>
        protected override void Receive(SLAM2DPoseMessage slamPoseMessage, GroundTruthMessage groundTruthMessage)
        {
            this.ProcessSlamAndGroundTruthMessages(slamPoseMessage, groundTruthMessage);
        }

        /// <summary>
        /// Method for processing a synchronized pair of slam and ground truth messages into metrics
        /// </summary>
        /// <param name="slamPoseMessage">A slam message</param>
        /// <param name="groundTruthMessage">A ground truth message</param>
        /// <param name="isLoopClosure">Indicates if this is a loop closure pair of messages</param>
        private void ProcessSlamAndGroundTruthMessages(SLAM2DPoseMessage slamPoseMessage, GroundTruthMessage groundTruthMessage, bool isLoopClosure = false)
        {
            Pose2D slamStartPose = new Pose2D();
            Pose2D slamMiddlePose = new Pose2D();
            Pose2D slamEndPose = slamPoseMessage.Location;

            Pose2D groundTruthStartPose = new Pose2D();
            bool isFirst = !this.referencedPoses.ContainsKey(groundTruthMessage.StartPoseId);
            bool isSecond = false;

            // find the referenced pose pairs, if they exists
            if (!isFirst)
            {
                Tuple<Pose2D, GroundTruthMessage> referencedPose = this.referencedPoses[groundTruthMessage.StartPoseId];
                slamMiddlePose = referencedPose.Item1;
                GroundTruthMessage message = referencedPose.Item2;
                groundTruthStartPose = message.DeltaPose;

                isSecond = !this.referencedPoses.ContainsKey(referencedPose.Item2.StartPoseId);
                if (!isSecond)
                {
                    referencedPose = this.referencedPoses[message.StartPoseId];
                    slamStartPose = referencedPose.Item1;
                }
                else
                {
                    // retain the poses, but not the particles to avoid GC issues (since they contain maps)
                    this.secondSlamMessage = new SLAM2DPoseMessage(slamPoseMessage.Location, slamPoseMessage.Poses, slamPoseMessage.OriginatingTime);
                    this.secondGroundTruthMessage = groundTruthMessage;
                }
            }

            // remember the current pose pair
            this.referencedPoses[groundTruthMessage.PoseId] = new Tuple<Pose2D, GroundTruthMessage>(slamEndPose, groundTruthMessage);

            // need at least one previous message to start publishing metrics
            if (!isFirst)
            {
                // Loop closure metric is only relevant in the 3-point Jalobeanu method.
                if (!isLoopClosure)
                {
                    this.PublishMetrics((pose) => ComputeError(slamMiddlePose, pose, groundTruthMessage), string.Empty, slamPoseMessage, groundTruthMessage);
                }

                // need at least two previous messages to start publishing 3-point Jalobeanu Metrics
                if (!isSecond)
                {
                    this.PublishMetrics((pose) => ComputeJalobeanuError(slamStartPose, slamMiddlePose, pose, groundTruthStartPose, groundTruthMessage.DeltaPose), "Bkr", slamPoseMessage, groundTruthMessage, isLoopClosure);
                }
            }

            this.lastGroundTruthPoseId = groundTruthMessage.PoseId;
        }

        /// <summary>
        /// Calculate the SLAM metrics and publish results
        /// </summary>
        /// <param name="errorFunc">The error function</param>
        /// <param name="metricPrefix">Prefix to prepend to the metric name</param>
        /// <param name="slamPoseMessage">The SLAM message</param>
        /// <param name="groundTruthMessage">The ground truth message</param>
        /// <param name="isLoopClosure">Indicates if this is a loop closure segment</param>
        private void PublishMetrics(Func<Pose2D, Tuple<double, double>> errorFunc, string metricPrefix, SLAM2DPoseMessage slamPoseMessage, GroundTruthMessage groundTruthMessage, bool isLoopClosure = false)
        {
            GaussianEstimator positioningGaussianEstimator = new GaussianEstimator();
            GaussianEstimator rotationGaussianEstimator = new GaussianEstimator();

            // get the Error and Precision
            double positioningError;
            double rotationError;

            Tuple<double, double> errors = errorFunc(slamPoseMessage.Location);
            positioningError = errors.Item1;
            rotationError = errors.Item2;

            foreach (Pose2D pose in slamPoseMessage.Poses)
            {
                errors = errorFunc(pose);
                positioningGaussianEstimator.Add(errors.Item1);
                rotationGaussianEstimator.Add(errors.Item2);
            }

            // expected distance/rotation
            double groundTruthX = groundTruthMessage.DeltaPose.X;
            double groundTruthY = groundTruthMessage.DeltaPose.Y;
            double expectedDistance = Math.Sqrt((groundTruthX * groundTruthX) + (groundTruthY * groundTruthY));
            double expectedRotation = Math.Abs(groundTruthMessage.DeltaPose.Heading);

            // publish the result
            Gaussian positioningGaussian = new Gaussian();
            positioningGaussian = positioningGaussianEstimator.GetDistribution(positioningGaussian);
            double positionAccuracy = positioningError / expectedDistance;
            double positionPrecision = Math.Sqrt(positioningGaussian.GetVariance()) / expectedDistance;
            MetricsResultMessage resultMessage = new MetricsResultMessage(metricPrefix + "Position", groundTruthMessage.PoseId, positionAccuracy, positionPrecision, expectedDistance, null, 0, null, this.runId, groundTruthMessage);
            this.Publisher.Post(resultMessage);

            Gaussian rotationGaussian = new Gaussian();
            rotationGaussian = rotationGaussianEstimator.GetDistribution(rotationGaussian);
            double rotationAccuracy = (expectedRotation == 0) ? 0 : rotationError / expectedRotation;
            double rotationPrecision = (expectedRotation == 0) ? 0 : Math.Sqrt(positioningGaussian.GetVariance()) / expectedRotation;
            resultMessage = new MetricsResultMessage(metricPrefix + "Orientation", groundTruthMessage.PoseId, rotationAccuracy, rotationPrecision, expectedRotation, null, 0, null, this.runId, groundTruthMessage, isLoopClosure);
            this.Publisher.Post(resultMessage);
        }
    }
}
